/****************************************************************
 * Fulguro
 * Copyright (C) 2004 Christophe Clienti
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 ***************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#ifdef __SSE2__
#include <emmintrin.h>
#include <xmmintrin.h>
#endif

#ifdef __MMX__
#include <mmintrin.h>
#endif

#include "flgrCoreDispatch.h"
#include "flgrCoreTransposeFast.h"
#include "flgrCoreTransposeFastUtil.h"

#ifdef __SSE2__
#define BYTE_BLOCK_SIZE 16
#define WORD_BLOCK_SIZE 8
#define DWORD_BLOCK_SIZE 4
#else
#define BYTE_BLOCK_SIZE 8
#define WORD_BLOCK_SIZE 4
#define DWORD_BLOCK_SIZE 2
#endif


void flgr_transp_block_byte(FLGR_Data2D *dest, FLGR_Data2D *src, int fromRow, int fromCol, int toRow, int toCol) {
#ifdef __SSE2__
  __m128i r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,r13,r14,r15,r16,tmp1;
  
  r1 = *((__m128i*) ( src->array[fromRow]+fromCol ));
  r2 = *((__m128i*) ( src->array[fromRow+1]+fromCol ));
  r3 = *((__m128i*) ( src->array[fromRow+2]+fromCol ));
  r4 = *((__m128i*) ( src->array[fromRow+3]+fromCol ));
  r5 = *((__m128i*) ( src->array[fromRow+4]+fromCol ));
  r6 = *((__m128i*) ( src->array[fromRow+5]+fromCol ));
  r7 = *((__m128i*) ( src->array[fromRow+6]+fromCol ));
  r8 = *((__m128i*) ( src->array[fromRow+7]+fromCol ));
  r9 = *((__m128i*) ( src->array[fromRow+8]+fromCol ));
  r10 = *((__m128i*) ( src->array[fromRow+9]+fromCol ));
  r11 = *((__m128i*) ( src->array[fromRow+10]+fromCol ));
  r12 = *((__m128i*) ( src->array[fromRow+11]+fromCol ));
  r13 = *((__m128i*) ( src->array[fromRow+12]+fromCol ));
  r14 = *((__m128i*) ( src->array[fromRow+13]+fromCol ));
  r15 = *((__m128i*) ( src->array[fromRow+14]+fromCol ));
  r16 = *((__m128i*) ( src->array[fromRow+15]+fromCol ));

  FLGR_MACRO_TRANSPOSE_BLOCK_16x16_BYTE(r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,r13,r14,r15,r16,tmp1);

  *((__m128i*) ( dest->array[toRow+0]+toCol )) = r16;
  *((__m128i*) ( dest->array[toRow+1]+toCol )) = r8;
  *((__m128i*) ( dest->array[toRow+2]+toCol )) = r12;
  *((__m128i*) ( dest->array[toRow+3]+toCol )) = r4;
  *((__m128i*) ( dest->array[toRow+4]+toCol )) = r14;
  *((__m128i*) ( dest->array[toRow+5]+toCol )) = r6;
  *((__m128i*) ( dest->array[toRow+6]+toCol )) = r10;
  *((__m128i*) ( dest->array[toRow+7]+toCol )) = r2;
  *((__m128i*) ( dest->array[toRow+8]+toCol )) = r15;
  *((__m128i*) ( dest->array[toRow+9 ]+toCol )) = r7;
  *((__m128i*) ( dest->array[toRow+10]+toCol )) = r11;
  *((__m128i*) ( dest->array[toRow+11]+toCol )) = r3;
  *((__m128i*) ( dest->array[toRow+12]+toCol )) = r13;
  *((__m128i*) ( dest->array[toRow+13]+toCol )) = r5;
  *((__m128i*) ( dest->array[toRow+14]+toCol )) = r9;
  *((__m128i*) ( dest->array[toRow+15]+toCol )) = r1;

#else
#ifdef __MMX__
  __m64 r1,r2,r3,r4,r5,r6,r7,r8,tmp1;

  r1 = *((__m64*) ( src->array[fromRow]+fromCol ));
  r2 = *((__m64*) ( src->array[fromRow+1]+fromCol ));
  r3 = *((__m64*) ( src->array[fromRow+2]+fromCol ));
  r4 = *((__m64*) ( src->array[fromRow+3]+fromCol ));
  r5 = *((__m64*) ( src->array[fromRow+4]+fromCol ));
  r6 = *((__m64*) ( src->array[fromRow+5]+fromCol ));
  r7 = *((__m64*) ( src->array[fromRow+6]+fromCol ));
  r8 = *((__m64*) ( src->array[fromRow+7]+fromCol ));

  FLGR_MACRO_TRANSPOSE_BLOCK_8x8_BYTE(tmp1,r1,r2,r3,r4,r5,r6,r7,r8);

  *((__m64*) ( dest->array[toRow]+toCol )) = r8;
  *((__m64*) ( dest->array[toRow+1]+toCol )) = r4;
  *((__m64*) ( dest->array[toRow+2]+toCol )) = r6;
  *((__m64*) ( dest->array[toRow+3]+toCol )) = r2;
  *((__m64*) ( dest->array[toRow+4]+toCol )) = r7;
  *((__m64*) ( dest->array[toRow+5]+toCol )) = r3;
  *((__m64*) ( dest->array[toRow+6]+toCol )) = r5;
  *((__m64*) ( dest->array[toRow+7]+toCol )) = r1;

  _mm_empty();
#else
  POST_ERROR("MMX/SSE2 Instruction not supported!\n");
#endif
#endif
}


void flgr_transp_block_word(FLGR_Data2D *dest, FLGR_Data2D *src, int fromRow, int fromCol, int toRow, int toCol) {
#ifdef __SSE2__
  __m128i r1,r2,r3,r4,r5,r6,r7,r8,tmp1;
  int fcol = fromCol*2;
  int tcol = toCol*2;
  
  r1 = *((__m128i*) ( src->array[fromRow]+fcol ));
  r2 = *((__m128i*) ( src->array[fromRow+1]+fcol ));
  r3 = *((__m128i*) ( src->array[fromRow+2]+fcol ));
  r4 = *((__m128i*) ( src->array[fromRow+3]+fcol ));
  r5 = *((__m128i*) ( src->array[fromRow+4]+fcol ));
  r6 = *((__m128i*) ( src->array[fromRow+5]+fcol ));
  r7 = *((__m128i*) ( src->array[fromRow+6]+fcol ));
  r8 = *((__m128i*) ( src->array[fromRow+7]+fcol ));

  FLGR_MACRO_TRANSPOSE_BLOCK_8x8_WORD(r1,r2,r3,r4,r5,r6,r7,r8,tmp1);

  *((__m128i*) ( dest->array[toRow+0]+tcol )) = r8;
  *((__m128i*) ( dest->array[toRow+1]+tcol )) = r4;
  *((__m128i*) ( dest->array[toRow+2]+tcol )) = r6;
  *((__m128i*) ( dest->array[toRow+3]+tcol )) = r2;
  *((__m128i*) ( dest->array[toRow+4]+tcol )) = r7;
  *((__m128i*) ( dest->array[toRow+5]+tcol )) = r3;
  *((__m128i*) ( dest->array[toRow+6]+tcol )) = r5;
  *((__m128i*) ( dest->array[toRow+7]+tcol )) = r1;
 
#else
  POST_ERROR("SSE2 Instruction not supported!\n");
#endif
}




void flgr_transp_block_dword(FLGR_Data2D *dest, FLGR_Data2D *src, int fromRow, int fromCol, int toRow, int toCol) {
#ifdef __SSE2__
  __m128i r1,r2,r3,r4,tmp1;
  int fcol = fromCol*4;
  int tcol = toCol*4;
  
  r1 = *((__m128i*) ( src->array[fromRow]+fcol ));
  r2 = *((__m128i*) ( src->array[fromRow+1]+fcol ));
  r3 = *((__m128i*) ( src->array[fromRow+2]+fcol ));
  r4 = *((__m128i*) ( src->array[fromRow+3]+fcol ));

  FLGR_MACRO_TRANSPOSE_BLOCK_4x4_DWORD(r1,r2,r3,r4,tmp1);

  *((__m128i*) ( dest->array[toRow+0]+tcol )) = r4;
  *((__m128i*) ( dest->array[toRow+1]+tcol )) = r2;
  *((__m128i*) ( dest->array[toRow+2]+tcol )) = r3;
  *((__m128i*) ( dest->array[toRow+3]+tcol )) = r1;
 
#else
  POST_ERROR("SSE2 Instruction not supported!\n");
#endif
}






void flgr2d_transpose_fast_fgUINT8(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc) {
  int i,j;

  FLGR_DECLARE_FUNCTION;

  for(i=0 ; i<imgsrc->size_y ; i+=BYTE_BLOCK_SIZE) {
    for(j=0 ; j<imgsrc->size_x ; j+=BYTE_BLOCK_SIZE) {
      flgr_transp_block_byte(imgdest,imgsrc,i,j,j,i);
    }
  }

}

void flgr2d_transpose_fast_fgUINT16(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc) {
  int i,j;

  FLGR_DECLARE_FUNCTION;

  for(i=0 ; i<imgsrc->size_y ; i+=WORD_BLOCK_SIZE) {
    for(j=0 ; j<imgsrc->size_x ; j+=WORD_BLOCK_SIZE) {
      flgr_transp_block_word(imgdest,imgsrc,i,j,j,i);
    }
  }

}

void flgr2d_transpose_fast_fgUINT32(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc) {
  int i,j;

  FLGR_DECLARE_FUNCTION;

  for(i=0 ; i<imgsrc->size_y ; i+=DWORD_BLOCK_SIZE) {
    for(j=0 ; j<imgsrc->size_x ; j+=DWORD_BLOCK_SIZE) {
      flgr_transp_block_dword(imgdest,imgsrc,i,j,j,i);
    }
  }

}
